#include "MOTOR/motor_app.h"
#include <math.h>
#include <stdlib.h>
/**
 ****************************************************************************************************
 * DONE 用户定义区
 ****************************************************************************************************
 */



/**
 ****************************************************************************************************
 * DONE 初始化区
 ****************************************************************************************************
 */

/**
 * @name 电机初始化
 * @brief 开启pwm
 */
void MOTOR_Init(void)
{
    HAL_TIM_PWM_Start(&htim3,TIM_CHANNEL_1);
    HAL_TIM_PWM_Start(&htim3,TIM_CHANNEL_2);
    HAL_TIM_PWM_Start(&htim3,TIM_CHANNEL_3);
    HAL_TIM_PWM_Start(&htim3,TIM_CHANNEL_4);

    HAL_TIM_PWM_Start(&htim4,TIM_CHANNEL_1);
    HAL_TIM_PWM_Start(&htim4,TIM_CHANNEL_2);
    HAL_TIM_PWM_Start(&htim4,TIM_CHANNEL_3);
    HAL_TIM_PWM_Start(&htim4,TIM_CHANNEL_4);

}

/**
 ****************************************************************************************************
 * DONE 系统函数
 ****************************************************************************************************
 */

/**
 * @name 电机控制函数
 * @brief 控制电机方向和速度
 * @param wheel_ID 电机ID，WHEEL_ID 类型
 * @param dir 电机运行方向，1为正转，0为反转，正反相对于车前进方向
 * @param speed 设置速度值
 */
void MOTOR_SetValue(WHEEL_ID wheel_ID,uint8_t dir,uint16_t speed)
{
    switch (wheel_ID)
    {
    case LF:if(dir) {__HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_1,speed);__HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_2,0);}
            else {__HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_1,0);__HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_2,speed);}
        break;
    case RB:if(dir) {__HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_3,speed);__HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_4,0);}
            else {__HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_3,0);__HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_4,speed);}
        break;
    case LB:if(dir) {__HAL_TIM_SET_COMPARE(&htim4,TIM_CHANNEL_1,speed);__HAL_TIM_SET_COMPARE(&htim4,TIM_CHANNEL_2,0);}
            else {__HAL_TIM_SET_COMPARE(&htim4,TIM_CHANNEL_1,0);__HAL_TIM_SET_COMPARE(&htim4,TIM_CHANNEL_2,speed);}
        break;
    case RF:if(dir) {__HAL_TIM_SET_COMPARE(&htim4,TIM_CHANNEL_3,speed);__HAL_TIM_SET_COMPARE(&htim4,TIM_CHANNEL_4,0);}
            else {__HAL_TIM_SET_COMPARE(&htim4,TIM_CHANNEL_3,0);__HAL_TIM_SET_COMPARE(&htim4,TIM_CHANNEL_4,speed);}
        break;
    default:
        break;
    }
}

void LF_Control(int Speed)
{
	if(Speed>0)
    {
		__HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_1, Speed);
		__HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_2, 0);
	}
	else
    {
		__HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_1, 0);
		__HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_2, -Speed);
	}
}

void LB_Control(int Speed)
{
	if(Speed>0)
    {
		__HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_1, Speed);
		__HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_2, 0);
	}
	else
    {
		__HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_1, 0);
		__HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_2, -Speed);
	}
}

void RB_Control(int Speed)
{
	if(Speed>0)
    {
		__HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_3, Speed);
		__HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_4, 0);
	}
	else
    {
		__HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_3, 0);
		__HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_4, -Speed);
	}
}


void RF_Control(int Speed)
{
	if(Speed>0)
    {
		__HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_3, Speed);
		__HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_4, 0);
	}
	else
    {
		__HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_3, 0);
		__HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_4, -Speed);
	}
}

/**
 ****************************************************************************************************
 * DONE 用户函数区
 ****************************************************************************************************
 */

#if MOTOR_CONTROL_MODE == 0
 
/**
 * @name 小车控制函数
 * @brief 控制小车前进
 */
void CAR_FRONT(uint16_t speed)
{
    MOTOR_SetValue(LF,1,speed);
    MOTOR_SetValue(LB,1,speed);
    MOTOR_SetValue(RF,1,speed);
    MOTOR_SetValue(RB,1,speed);
}

/**
 * @name 小车控制函数
 * @brief 控制小车后退
 */
void CAR_BACK(uint16_t speed)
{
    MOTOR_SetValue(LF,0,speed);
    MOTOR_SetValue(LB,0,speed);
    MOTOR_SetValue(RF,0,speed);
    MOTOR_SetValue(RB,0,speed);
}

/**
 * @name 小车控制函数
 * @brief 控制小车差速左转
 */
void CAR_LEFT(uint16_t speed)
{
    MOTOR_SetValue(LF,0,0);
    MOTOR_SetValue(LB,0,0);
    MOTOR_SetValue(RF,1,speed);
    MOTOR_SetValue(RB,1,speed);
}

/**
 * @name 小车控制函数
 * @brief 控制小车原地左转
 */
void CAR_TURN_LEFT(uint16_t speed)
{
    MOTOR_SetValue(LF,0,speed);
    MOTOR_SetValue(LB,0,speed);
    MOTOR_SetValue(RF,1,speed);
    MOTOR_SetValue(RB,1,speed);
}

/**
 * @name 小车控制函数
 * @brief 控制小车差速右转
 */
void CAR_RIGHT(uint16_t speed)
{
    MOTOR_SetValue(LF,1,speed);
    MOTOR_SetValue(LB,1,speed);
    MOTOR_SetValue(RF,0,0);
    MOTOR_SetValue(RB,0,0);
}

/**
 * @name 小车控制函数
 * @brief 控制小车原地右转
 */
void CAR_TURN_RIGHT(uint16_t speed)
{
    MOTOR_SetValue(LF,1,speed);
    MOTOR_SetValue(LB,1,speed);
    MOTOR_SetValue(RF,0,speed);
    MOTOR_SetValue(RB,0,speed);
}

/**
 * @name 小车控制函数
 * @brief 控制小车停止
 */
void CAR_STOP(uint16_t speed)
{
    MOTOR_SetValue(LF,0,0);
    MOTOR_SetValue(LB,0,0);
    MOTOR_SetValue(RF,0,0);
    MOTOR_SetValue(RB,0,0);
}

#elif MOTOR_CONTROL_MODE == 1

/**
 * @name 小车控制函数
 * @brief 控制小车前进
 */
void CAR_FRONT(uint16_t speed)
{
    LF_Control(speed);
    LB_Control(speed);
    RF_Control(speed);
    RB_Control(speed);
}

/**
 * @name 小车控制函数
 * @brief 控制小车后退
 */
void CAR_BACK(uint16_t speed)
{
    LF_Control(-speed);
    LB_Control(-speed);
    RF_Control(-speed);
    RB_Control(-speed);
}

/**
 * @name 小车控制函数
 * @brief 控制小车差速左转
 */
void CAR_LEFT(uint16_t speed)
{
    LF_Control(0);
    LB_Control(0);
    RF_Control(speed);
    RB_Control(speed);
}

/**
 * @name 小车控制函数
 * @brief 控制小车原地左转
 */
void CAR_TURN_LEFT(uint16_t speed)
{
    LF_Control(-speed);
    LB_Control(-speed);
    RF_Control(speed);
    RB_Control(speed);
}

/**
 * @name 小车控制函数
 * @brief 控制小车差速右转
 */
void CAR_RIGHT(uint16_t speed)
{
    LF_Control(speed);
    LB_Control(speed);
    RF_Control(0);
    RB_Control(0);
}

/**
 * @name 小车控制函数
 * @brief 控制小车原地右转
 */
void CAR_TURN_RIGHT(uint16_t speed)
{
    LF_Control(speed);
    LB_Control(speed);
    RF_Control(-speed);
    RB_Control(-speed);
}

/**
 * @name 小车控制函数
 * @brief 控制小车停止
 */
void CAR_STOP(uint16_t speed)
{
    LF_Control(0);
    LB_Control(0);
    RF_Control(0);
    RB_Control(0);
}

#endif


/**
 * @name 循迹小车控制
 * @brief 控制循迹小车巡线
 * @param speed 速度值
 */
void Track_Car(uint16_t speed)
{
    Track_Area temp_dir = SENSOR_Track_Dispose();
    if(speed < 400) speed = 400;
    switch (temp_dir)
    {
    case Track_B:   CAR_BACK(speed-240);    //超出 后退
    break;
    case Track_LLL: CAR_TURN_LEFT(speed);   //左外 左转
    break;
    case Track_LL:  CAR_LEFT(speed-120);    //左   左转
    break;
    case Track_L:   CAR_LEFT(speed-240);    //中左 左转
    break;  
    case Track_C:   CAR_FRONT(speed-200);   //中   前进
    break;
    case Track_R:   CAR_RIGHT(speed-240);   //中右 右转
    break;
    case Track_RR:  CAR_RIGHT(speed-120);   //右   右转
    break;
    case Track_RRR: CAR_TURN_RIGHT(speed);  //右外 右转
    break;
    case Track_S:   CAR_STOP(500);          
    break;
    default:        CAR_STOP(500);
        break;
    }
}


